#include <Arduino.h>
#include "Motor.h"
#include "color.h"
#include "Servo.h"

#include <SCoop.h>//引入头文件


int setPoint = 0;
int acceleration;
int x;
int y;
char ch;
int a = 0;
int b = 0;

int gray_two = 0;

int tuoluoyi;

int bubu = 0;
int kk = 0;
int wuwu = 0;
int wuwu1 = 0;
int wuwu2 = 0;
int wuwu3 = 0;
int wuwu4 = 0;
int wuwu5 = 0;

int Filter_Value;

int tuoluoyi_origin = 580;   // 初始角度 初始化
int tuoluoyi_origin_F = 528;
int tuoluoyi_origin_F_taishang = 528;

int tuoluoyi_origin2 = 470;  //后组架倾斜角度
int tuoluoyi_origin_F_shang = 660;  // 前组架上坡角度
int tuoluoyi_origin_F_xia = 412;   // 前组架下坡角度

int P = 0;
int pi = 0.8;   // 平地pid
int pi2 = 1.5;  // 上坡pid
int pi3 = 1.5;  // 特殊pid

int Get_AD() {
  return analogRead(A5);
}

int Get_AD2() {
  return analogRead(A0);
}

#define FILTER_N 10
int Filter() {
  int i, j;
  int filter_temp, filter_sum = 0;
  int filter_buf[FILTER_N];
  for(i = 0; i < FILTER_N; i++) {
    filter_buf[i] = Get_AD();
    delay(1);
  }
  // 采样值从小到大排列（冒泡法）
  for(j = 0; j < FILTER_N - 1; j++) {
    for(i = 0; i < FILTER_N - 1 - j; i++) {
      if(filter_buf[i] > filter_buf[i + 1]) {
        filter_temp = filter_buf[i];
        filter_buf[i] = filter_buf[i + 1];
        filter_buf[i + 1] = filter_temp;
      }
    }
  }
  // 去除最大最小极值后求平均
  for(i = 1; i < FILTER_N - 1; i++) filter_sum += filter_buf[i];
  return filter_sum / (FILTER_N - 2);
}

int Filter2() {
  int i, j;
  int filter_temp, filter_sum = 0;
  int filter_buf[FILTER_N];
  for(i = 0; i < FILTER_N; i++) {
    filter_buf[i] = Get_AD2();
    delay(1);
  }
  // 采样值从小到大排列（冒泡法）
  for(j = 0; j < FILTER_N - 1; j++) {
    for(i = 0; i < FILTER_N - 1 - j; i++) {
      if(filter_buf[i] > filter_buf[i + 1]) {
        filter_temp = filter_buf[i];
        filter_buf[i] = filter_buf[i + 1];
        filter_buf[i + 1] = filter_temp;
      }
    }
  }
  // 去除最大最小极值后求平均
  for(i = 1; i < FILTER_N - 1; i++) filter_sum += filter_buf[i];
  return filter_sum / (FILTER_N - 2);
}

void heixian(){
  left_val=analogRead(left_pin);
  right_val=analogRead(right_pin);
  if(left_val < 750  && right_val < 750 ){
  gray_two ++ ;
}
}

void heixian2(){
  while(bubu == 0){
  time = millis();
  bubu = 1;
  }
  int time2 = 0;
  time2 = millis();
  left_val=analogRead(left_pin);
  right_val=analogRead(right_pin);
  if(left_val < 750  && right_val < 750 ){
  gray_two ++ ;
} 
  if((time2 - time)>5000){
   gray_two++;
   }
}


void tuoluoyi_panduan_shangpo_pingdi(){
  
  tuoluoyi = Filter2();

    if(tuoluoyi > 480 && tuoluoyi < 560){
      gray_two++;
  }
}

void tuoluoyi_panduan_shangpo_xiapo(){
  
  tuoluoyi = Filter2();

    if(tuoluoyi > 380 && tuoluoyi < 450){
      gray_two++;
  }
}


void tuoluoyi_PID(){   //从斜到平
  int error = 0;
  int tuoluoyi_final = 0;
  int speed = 0;
  tuoluoyi_final = Filter();
  error = tuoluoyi_origin - tuoluoyi_final;
  if(error < 30){
    P = 255;
    speed = 255;
  }
  else{
  P = 60 + error*pi3;
  }
    if(P > 255){
    P = 255;
  }
  analogWrite(LFwheel_1,P);//-55
  analogWrite(LFwheel_2,speed);
  analogWrite(RFwheel_1,P);
  analogWrite(RFwheel_2,speed);
}

void tuoluoyi_PID_F(){   //从平到斜
  int error = 0;
  int tuoluoyi_final = 0;
  int speed = 0;
  tuoluoyi_final = Filter2();
  error =  tuoluoyi_origin_F_shang - tuoluoyi_final;
  if(error < 30){
    P = 255;
    speed = 255;
   
  }
  else{
  P = 60 + error*pi2;
  }
  if(P > 255){
    P = 255;
  }
  if(P < 180){
    P = 180;
  }
  analogWrite(LFwheel_1,P);//-55
  analogWrite(LFwheel_2,speed);
  analogWrite(RFwheel_1,P);
  analogWrite(RFwheel_2,speed);
}

void tuoluoyi_PID_F2(){   //从平
  int error = 0;
  int tuoluoyi_final = 0;
  int speed = 0;
  tuoluoyi_final = Filter2();
  error =  tuoluoyi_final - 520;
  if(error < 30){
    P = 255;
    speed = 255;
  }
  else{
  P = 30 + error*pi;
  }
  if(P > 255){
    P = 255;
  }
    if(P < 150){
    P = 150;
  }
  analogWrite(LFwheel_1,P);//-55
  analogWrite(LFwheel_2,speed);
  analogWrite(RFwheel_1,P);
  analogWrite(RFwheel_2,speed);
}

void tuoluoyi_PID_B(){   //从平
  int error = 0;
  int tuoluoyi_final = 0;
  int speed = 0;
  tuoluoyi_final = Filter2();
  error =  tuoluoyi_origin_F - tuoluoyi_final;
  if(error < 30){
    P = 255;
    speed = 255;
  }
  else{
  P = 30 + error*pi;
  }
  if(P > 255){
    P = 255;
  }
    if(P < 150){
    P = 150;
  }
  analogWrite(LFwheel_1,P);//-55
  analogWrite(LFwheel_2,speed);
  analogWrite(RFwheel_1,P);
  analogWrite(RFwheel_2,speed);
}

void tuoluoyi_PID_B2(){   //从平
  int error = 0;
  int tuoluoyi_final = 0;
  int speed = 0;
  tuoluoyi_final = Filter2();
  error =  tuoluoyi_final - 520;
  if(error < 30){
    P = 255;
    speed = 255;
  }
  else{
  P = 30 + error*pi;
  }
  if(P > 255){
    P = 255;
  }
    if(P < 100){
    P = 100;
  }
  analogWrite(LFwheel_1,P);//-55
  analogWrite(LFwheel_2,speed);
  analogWrite(RFwheel_1,P);
  analogWrite(RFwheel_2,speed);
}


void shangtaijie(){
for(int i = 0; i < 250; i++){
   tuoluoyi_PID_F();
}

for(int i = 0; i < 250; i++){
   tuoluoyi_PID_F2();
}

taijie_put();
delay(2000);

for(int i = 0; i < 250; i++){
   tuoluoyi_PID_F();
}
for(int i = 0; i < 250; i++){
   tuoluoyi_PID_F2();
}
for(int i = 0; i < 250; i++){
   tuoluoyi_PID();
}

gray_two++;
}


defineTaskLoop(Task1)//快速定义 省去setup
{
  // int yuntai_jiaodu = 0;
  // tuoluoyi = Filter();
  // yuntai_jiaodu = tuoluoyi_origin - tuoluoyi;
  // yuntai_jiaodu = map(yuntai_jiaodu,-100,100,20,60);
  // servo_con(yuntai_jiaodu);
  yuntaijiaodu(1100);
}


void yuntai(){
  int yuntai_jiaodu = 0;
  tuoluoyi = Filter();
  yuntai_jiaodu = tuoluoyi_origin - tuoluoyi;
  yuntai_jiaodu = map(yuntai_jiaodu,-100,100,20,60);
  servo_con(yuntai_jiaodu);
}




void huan(){

for(int i = 50; i < 500; i = i + 2){

  int tuoluoyi_final2 = 0;
  tuoluoyi_final2 = Filter2();

  int speed1 = i+20;
  int speed2 = i;

  if(speed1>140){
    speed1 = 140;
  }
    if(speed2>150){
    speed2 = 150;
  }

  analogWrite(LFwheel_1,speed1);
  analogWrite(LFwheel_2,0);
  analogWrite(RFwheel_1,speed2);
  analogWrite(RFwheel_2,0);

  if(tuoluoyi_final2 < 480){  //1 480  2 480
    stop();
    i = 500;
    gray_two++;
  }
  delay(20);
}
}
void huan2(){
for(int i = 20; i < 500; i = i + 1){

  int tuoluoyi_final2 = 0;
  tuoluoyi_final2 = Filter2();

  int speed1 = i+20;
  int speed2 = i;

  if(speed1>90){
    speed1 = 90;
  }
    if(speed2>120){
    speed2 = 120;
  }

  analogWrite(LFwheel_1,speed1);
  analogWrite(LFwheel_2,0);
  analogWrite(RFwheel_1,speed2);
  analogWrite(RFwheel_2,0);

  if(tuoluoyi_final2 < 540 && tuoluoyi_final2 > 520){  //1 480  2 480
    stop();
    i = 500;
    gray_two++;
  }
  delay(20);
}
}

void xiajiang(int jiao){
  
  for(int i = 0;i < 16;i++){
  servo_con2(jiao);
  jiao = jiao - 5;
  delay(300);
  }
}

void xiajiang2(int jiao){
  
  for(int i = 0;i < 10;i++){
  servo_con(jiao);
  jiao = jiao - 5;
  delay(300);
  }
}

void pingdipanduan(){
    int tuoluoyi_final2 = 0;
  tuoluoyi_final2 = Filter2();
    if(tuoluoyi_final2 < 540 && tuoluoyi_final2 > 520){  //1 480  2 480
    stop();
    
    gray_two++;
  }
}

void pingdipanduan2(){
    int tuoluoyi_final2 = 0;
  tuoluoyi_final2 = Filter2();
    if(tuoluoyi_final2 < 480){  //1 480  2 480
    stop();
    
    gray_two++;
  }
}

void panduan1(){
  int ppo = 0;
  ppo = Filter2();
  if(ppo > 620){
    stop();
    gray_two++;
  }
}

void shanglai1(){
for(int i = 50; i < 500; i = i + 1){

  int tuoluoyi_final2 = 0;
  tuoluoyi_final2 = Filter2();

  int speed1 = i+20;
  int speed2 = i;

  if(speed1>110){
    speed1 = 110;
  }
    if(speed2>150){
    speed2 = 150;
  }

  analogWrite(LFwheel_1,speed1);
  analogWrite(LFwheel_2,0);
  analogWrite(RFwheel_1,speed2);
  analogWrite(RFwheel_2,0);

  if(tuoluoyi_final2 < 620  && tuoluoyi_final2 > 610){  //1 480  2 480
    stop();
    i = 500;
    gray_two++;
  }
  delay(20);
}
}

void shanglai2(){
for(int i = 80; i < 500; i = i + 1){

  int tuoluoyi_final2 = 0;
  tuoluoyi_final2 = Filter2();

  int speed1 = i+20;
  int speed2 = i;

  if(speed1 > 130){
    speed1 = 130;
  }
    if(speed2 > 170){
    speed2 = 170;
  }

  analogWrite(LFwheel_1,speed1);
  analogWrite(LFwheel_2,0);
  analogWrite(RFwheel_1,speed2);
  analogWrite(RFwheel_2,0);

  if(tuoluoyi_final2 < 540 && tuoluoyi_final2 > 520){  //1 480  2 480
    stop();
    i = 500;
    gray_two++;
  }
  delay(20);
}
}

void setup() {
// put your setup code here, to run once:
  Serial.begin(9600);
  // Color_Init();
  Motor_Init();
  Servo_Init();
  pinMode(A5,INPUT);
  pinMode(12,OUTPUT);
  pinMode(7,OUTPUT);
	mySCoop.start();
  delay(100);
}



void loop() {

  while(kk == 0){
   tuoluoyi_origin = Filter();
   tuoluoyi_origin_F = Filter2();
   tuoluoyi_origin_F_taishang = Filter2();
   servo_con2(10);
   servo_con(10);
   delay(500);
   servo_con2(30);
   servo_con(30);
   delay(500);
   servo_con2(10);
   servo_con(10);
   delay(500);
   yuntaijiaodu(1100);
   kk = 1;
  }

  while(1){
    yield();
  switch(gray_two){
    case 0:  trace();heixian(); break;
    
    case 1:  stop();delay(1000);gray_two++;shangpo_high();delay(2000);break;
    case 2:  shangpo_high();tuoluoyi_panduan_shangpo_pingdi();delay(200);break;//上坡
    
    case 3:  shangpo_low();tuoluoyi_panduan_shangpo_xiapo(); break; //下坡
    case 4:  shangpo_low2();delay(2000);gray_two++; break;

    case 5:  drive_forward_1(1800);gray_two++;      break;// 过管道
    case 6:  trace();heixian2();      break;
    case 7:  stop();delay(1000);shangpo_low();delay(1800);gray_two++;break;

    case 8 : servo_con(100);taijie_low();delay(3000);gray_two++;break;  // 上台阶
    case 9 : servo_con(10); shanglai2();break;
    case 10 : servo_con2(100);  taijie_low();delay(3000);gray_two++;break;
    case 11 :  servo_con(100);delay(1000);servo_con2(10);delay(3000);servo_con(10);gray_two++;break;
    case 12 : shanglai2();

    case 13 : servo_con(80);servo_con2(80);delay(1000);gray_two++;break;// 下台阶
    case 14 : huan();break;
    case 15 : taijie_low();delay(500);stop();delay(500);xiajiang2(50);gray_two++;break;
    case 16 : huan2();break;
    case 17 : servo_con(80);delay(1000);gray_two++;break;
    case 18 : huan();break;
    case 19 : xiajiang(80);gray_two++;break;
    case 20 : xiajiang2(50);gray_two++;break;
    case 21 : huan2();break;
    case 22 : servo_con2(80);delay(1000);gray_two++;break;
    case 23 : taijie_low();delay(600);stop();gray_two++;break;
    case 24 : xiajiang(80);gray_two++;break;

    case 25:  drive_forward_1(2000);gray_two++;      break;  // 过管道
    case 26:  trace();heixian();      break;
    case 27:  stop(); gray_two++;     break;

    case 28 : break;

    default: trace();break;
  }
  tuoluoyi = Filter();
   Serial.println(tuoluoyi);
  Serial.println(gray_two);
}

//servo_con(80);
// servo_con2(90);
// xiajiang(90);
//Serial.println(Filter2());
//taijie_low();
//huan();
// servo_con(100);
// taijie_low();

// Serial.println(Filter2());
// while(1){
//   switch(gray_two){
//     case 0 : servo_con(100);taijie_low();delay(3000);gray_two++;
//     case 1 : servo_con(10); shanglai2();
//     case 2 : servo_con2(100);  taijie_low();delay(3000);gray_two++;
//     case 3:  servo_con(100);delay(1000);servo_con2(10);delay(3000);servo_con(10);gray_two++;
//     case 4 : shanglai2();

//     case 5 : servo_con(80);servo_con2(80);delay(1000);gray_two++;
//     case 6 : huan();break;
//     case 7 : taijie_low();delay(500);stop();delay(500);xiajiang2(50);gray_two++;
//     case 8 : huan2();break;
//     case 9 : servo_con(80);delay(1000);gray_two++;
//     case 10 : huan();
//     case 11 : xiajiang(80);gray_two++;
//     case 12 : xiajiang2(50);gray_two++;
//     case 13 : huan2();
//     case 14 : servo_con2(80);delay(1000);gray_two++;
//     case 15 : taijie_low();delay(600);stop();gray_two++;
//     case 16 : xiajiang(80);gray_two++;


//   }
// }
//  while(1){
//   switch(gray_two){
//     case 0 : servo_con(80);servo_con2(80);delay(1000);gray_two++;
//     case 1 : huan();break;
//     case 2 : taijie_low();delay(500);stop();delay(500);xiajiang2(50);gray_two++;
//     case 3 : huan2();break;
//     case 4 : servo_con(80);delay(1000);gray_two++;
//     case 5 : huan();
//     case 6 : xiajiang(80);gray_two++;
//     case 7 : xiajiang2(50);gray_two++;
//     case 8 : huan2();
//     case 9 : servo_con2(80);delay(1000);gray_two++;
//     case 10 : taijie_low();delay(600);stop();gray_two++;
//     case 11 : xiajiang(80);gray_two++;

//     // case  2 : servo_con(50);taijie_low();pingdipanduan();break;
//     // case  3 : taijie_low();delay(100);gray_two++;break;
//     // case 4 : taijie_low2();pingdipanduan2();break;
//     // case  5 : stop();break;
//     // case  6 : stop();break;
//   }
//  }

  // put your main code here, to run repeatedly:
}